/*
 * ImageConverter.cpp
 *
 *  Created on: 24 mai 2012
 *      Author: turtlebot
 */

#include "../include/unicebot_gui/ImageConverter.hpp"

// source: http://www.isep.ipp.pt/roswiki/cv_bridge(2f)Tutorials(2f)UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages.html

namespace unicebot_gui {

static const char WINDOW[] = "ImageConverter";

//static const char IMAGE_TOPIC_OUT[] = "out";

ImageConverter::ImageConverter() {
	ROS_WARN("KinectWidget::KinectWidget");
}

void ImageConverter::init(ros::NodeHandle nh, std::string topic_in, KinectWidget* pKinectWin ){
	ROS_WARN("ImageConverter::init");

	this->pKinectWin = pKinectWin;
	image_topic_in_ = topic_in;


	image_transport::ImageTransport it(nh);

	// S'aboner au topic (image entrante)
	//
	ROS_WARN("Subscribing to: ");
	ROS_WARN(image_topic_in_.c_str());

	image_sub_ = it.subscribe(image_topic_in_, 1, &ImageConverter::imageCb2, this);

	// Publier un topic (image sortante)
	//image_pub_ = it.advertise(IMAGE_TOPIC_OUT, 1);

	// Afficher une fenetre OpenCV
	//
	cv::namedWindow(WINDOW);

	cvStartWindowThread();
}

ImageConverter::~ImageConverter() {
	// Detruire la fenetre opencv
	//
	cv::destroyWindow(WINDOW);
}


cv_bridge::CvImagePtr ImageConverter::rosImageToCv(const sensor_msgs::ImageConstPtr& msg){
	ROS_WARN("ImageConverter::rosImageToCv");

	cv_bridge::CvImagePtr cv_ptr;
	try {
		cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	} catch (cv_bridge::Exception& e) {
		ROS_ERROR("cv_bridge exception: %s", e.what());
		return cv_ptr;
	}

	return cv_ptr;
}

void ImageConverter::drawCvImage(const cv_bridge::CvImagePtr cv_ptr){
	ROS_WARN("ImageConverter::drawCvImage");
	// Dessiner un cercle
	//
	if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
		cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255, 0, 0));

}

QImage* ImageConverter::cvImageToQt(const cv_bridge::CvImagePtr cv_ptr){
	ROS_WARN("ImageConverter::cvImageToQt");
	cv::Mat opencv_image = cv_ptr->image;
	cv::Mat dest;
	cvtColor(opencv_image, dest,CV_BGR2RGB);
	QImage image((uchar*)dest.data, dest.cols, dest.rows,QImage::Format_RGB888);

	return &image;

}



void ImageConverter::imageCb2(const sensor_msgs::ImageConstPtr& msg) {
	ROS_WARN("ImageConverter::imageCb2");


	ROS_WARN("####Encoding:");
	ROS_WARN(msg->encoding.c_str());

	// Convertir l'image ros en open_cv
	//
	cv_bridge::CvImagePtr cv_ptr = rosImageToCv(msg);

	// Dessiner sur l'image
	//
	drawCvImage(cv_ptr);

	//Convertir l'image opencv en Qt
	//
	// FIXME
	//pQtImg_ = cvImageToQt(cv_ptr); //marche pas
	pQtImg_ = Mat2QImage(cv_ptr->image); //marche moyen

	// Transmettre l'image à la fenetre Qt (copie)
	//
	ROS_WARN("mutex.lock");
	pKinectWin->mutex.lock();
	ROS_WARN("mutex.lock");
	//ROS_WARN(pQtImg_->isNull());
	pKinectWin->imageToDraw = *pQtImg_;
	ROS_WARN("mutex.unlock");
	pKinectWin->mutex.unlock();

    QRect *r = new QRect(0, 0, pKinectWin->width(), pKinectWin->height());
	//application->postEvent(pKinectWin, evt);

	//pKinectWin->repaint();


	// Afficher l'image sur la fenetre opencv
	//
	cv::imshow(WINDOW, cv_ptr->image);
	cv::waitKey(3);

	// Publier l'image (topic)
	//
	//image_pub_.publish(cv_ptr->toImageMsg());
}

QImage* ImageConverter::Mat2QImage(const cv::Mat_<double> &src)
{
        double scale = 255.0;
        QImage* dest = new QImage(src.cols, src.rows, QImage::Format_ARGB32);
        for (int y = 0; y < src.rows; ++y) {
                const double *srcrow = src[y];
                QRgb *destrow = (QRgb*)dest->scanLine(y);
                for (int x = 0; x < src.cols; ++x) {
                        unsigned int color = srcrow[x] * scale;
                        destrow[x] = qRgba(color, color, color, 255);
                }
        }
        return dest;
}


void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
	ROS_WARN("ImageConverter::imageCb");

  ROS_WARN("####Encoding:");
  //ROS_WARN(msg->encoding.c_str());
  //FIXMEimage_mutex_.lock();

  // May want to view raw bayer data, which CvBridge doesn't know about
  if (msg->encoding.find("bayer") != std::string::npos)
  {
    last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
                          const_cast<uint8_t*>(&msg->data[0]), msg->step);
  }
  // We want to scale floating point images so that they display nicely
  else if(msg->encoding.find("F") != std::string::npos)
  {

	  //ROS_WARN("####Floating:");
	  /*
	  	  cv::Mat  bridge;
	      try {
	        bridge = img_bridge_.imgMsgToCv(msg, "passthrough");
	      }
	      catch (sensor_msgs::CvBridgeException& e)
	      {
	          ROS_ERROR("Failed to transform depth image.");
	          return;
	      }

	      double min_range_=0.5;
	      double max_range_=5.5;

	      // convert to something visible
	      cv::Mat img(bridge.rows, bridge.cols, CV_8UC1);
	      for(int i = 0; i < bridge.rows; i++)
	      {
	          float* Di = bridge.ptr<float>(i);
	          char* Ii = img.ptr<char>(i);
	          for(int j = 0; j < bridge.cols; j++)
	          {
	              Ii[j] = (char) (255*((Di[j]-min_range_)/(max_range_-min_range_)));
	          }
	      }
    last_image_ = img;
    //*/


	    cv::Mat float_image_bridge = img_bridge_.imgMsgToCv(msg, "32FC1");
	    cv::Mat_<float> float_image = float_image_bridge;
	    float max_val = 0;
	    for(int i = 0; i < float_image.rows; ++i)
	    {
	      for(int j = 0; j < float_image.cols; ++j)
	      {
	        max_val = std::max(max_val, float_image(i, j));
	      }
	    }

	    if(max_val > 0)
	    {
	      float_image /= max_val;
	    }
	    last_image_ = float_image;
  }
  else
  {
    // Convert to OpenCV native BGR color
    try {
      last_image_ = img_bridge_.imgMsgToCv(msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException& e) {
    	ROS_ERROR("Unable to convert '%s' image to bgr8",
                             msg->encoding.c_str());

    }
  }

  // last_image_ may point to data owned by last_msg_, so we hang onto it for
  // the sake of mouseCb.
  last_msg_ = msg;

  // Must release the mutex before calling cv::imshow, or can deadlock against
  // OpenCV's window mutex.
  //FIXME image_mutex_.unlock();
  if (!last_image_.empty()){
    cv::imshow(WINDOW, last_image_);
    cv::waitKey(1);
  }
}

} //namespace unicebot_gui {
